#include "../handler_task.h"
#include "../../driver/pick/parts.h"
#include "../../driver/pick/pick_door.h"
#include "../../driver/pick/sensor.h"
#include "../../service/salve_conn.h"
#include "../../conn/proto.h"

namespace task {

using namespace driver;
using namespace conn;
using namespace service;
using namespace utils;

static Error on_init() {
    Error err;
    if (sensor->is_pick1_check()) {
        err = ec::PICK_DOOR1_CHECK;
        goto finish;
    } 

    err = pickDoor->close1();
    if (err) {
        goto finish;
    }

    if (sensor->is_pick2_check()) {
        err = ec::PICK_DOOR2_CHECK;
        goto finish;
    }

    err = pickDoor->close2();
    if (err) {
        goto finish;
    }

    err = pickDoor->close_straw();
    if (err) {
        goto finish;
    }
    parts->close_led1();
    parts->close_led2();
    parts->close_straw();
finish:
    return err;
}

static Error on_door_ctrl(const Message &msg) {
    uint8_t type, ctrl;
    uint16_t v;
    Error err = msg.parse(type, ctrl);
    if (err) {
        goto finish;
    }
    v = (type << 8) + ctrl;
    switch (v) {
        case 0x0100: err = pickDoor->open1(); break;
        case 0x0101: err = pickDoor->close1(); break;
        case 0x0200: err = pickDoor->open2(); break;
        case 0x0201: err = pickDoor->close2(); break;
        case 0x0300: err = pickDoor->open_straw(); break;
        case 0x0301: err = pickDoor->close_straw(); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

static Error on_led_ctrl(const Message &msg) {
    uint8_t type, ctrl;
    uint16_t v;
    Error err = msg.parse(type, ctrl);
    if (err) {
        goto finish;
    }
    v = (type << 8) + ctrl;
    switch (v) {
        case 0x0100: parts->open_led1(); break;
        case 0x0101: parts->close_led1(); break;

        case 0x0200: parts->open_led2(); break;
        case 0x0201: parts->close_led2(); break;

        case 0x0300: parts->open_straw(); break;
        case 0x0301: parts->close_straw(); break;

        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

template <uint8_t ID>
static Error pick() {
    Error err;

    static_assert(ID <= 2, "id FAIL");

    err = pickDoor->open_straw();
    if (err) {
        goto finish;
    }

    parts->open_straw();

    if constexpr (ID == 1) {
        parts->open_led1();
        err = pickDoor->open1();
        if (err) {
            goto finish;
        }
        if (!sensor->is_pick1_check()) {
            err = ec::PICK_DOOR1_LOSE;
            goto finish;
        }
        while (sensor->is_pick1_check()) {
            osDelay(500);
        }
        err = pickDoor->close1();
        if (err) {
            goto finish;
        }
        parts->close_led1();
    } else {
        parts->open_led2();
        err = pickDoor->open2();
        if (err) {
            goto finish;
        }
        if (!sensor->is_pick2_check()) {
            err = ec::PICK_DOOR2_LOSE;
            goto finish;
        }
        while (sensor->is_pick2_check()) {
            osDelay(500);
        }
        err = pickDoor->close2();
        if (err) {
            goto finish;
        }
        parts->close_led2();
    }
    osDelay(2000);

    err = pickDoor->close_straw();
    if (err) {
        goto finish;
    }

finish:
    return err;
}

static Error on_pick(const Message &msg) {
    uint8_t type;
    Error err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 1: err = pick<1>(); break;
        case 2: err = pick<2>(); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

void HandlerTask::dispatch_impl(const Message &msg) {
    Error err;

    switch (msg.conn_info.req) {
        
        case proto::REQ_PICK_INIT: 
            err = on_init();
        break;

        case proto::REQ_PICK_DOOR_CTRL:
            err = on_door_ctrl(msg);
        break;

        case proto::REQ_PICK_LED_CTRL:
            err = on_led_ctrl(msg);
        break;

        case proto::REQ_PICK_DO:
            err = on_pick(msg);
        break;

        default: err = ec::INVALID; break;
    }

finish:
    salveConn->response(msg, err);
}

}
